
#include "RobotJointPub.h"

RWJointPub::RWJointPub()
{

}

RWJointPub::~RWJointPub()
{

}

void RWJointPub::init(webots::Robot* Rin,ros::NodeHandle* nin,string name)
{
//  int roslaunch = fork();
//  if(roslaunch == 0)
//  {
//
//  }

  n = nin;
  R = Rin;

  joint_state_pub = n->advertise<sensor_msgs::JointState>("joint_states", 1000);
  RobJoints.header.seq = 0;

  //Add joint names here
  joint_names.push_back("lidar_jt");
  joint_names.push_back("lidar_optical_jt");
  joint_names.push_back("range_cam_jt");

  RobJoints.name.resize(joint_names.size());
  for(int i=0;i<=(int)joint_names.size();i++)
  {
    RobJoints.name[i] = joint_names[i];
  }
}

void RWJointPub::iterate()
{
  RobJoints.header.seq++;
  RobJoints.header.stamp = ros::Time::now();

  RobJoints.position[0] = 0;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  transform.setOrigin( tf::Vector3(0.0, 0.0, 0.5) );
  transform.setRotation( tf::Quaternion(0, 0,1, 1.57) );
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/base_lk"));

}
